## NOTES TO EDITORS
# todo: Implement New Gamecontroller
# TODO: Fix locategoal
# TODO: Fix doorlopen when penalized
# TODO: Cleaning visioninterface

## FILE DESCRIPTION
# File: soul.py
# Description: The nao's footballplaying soul.
# * states call phases
# * phases call moves
# * Transitions between states through global int state
# * Transitions between phases through global string phase
# state : e.g. 1
# phase : e.g. 'BallFound'

from naoqi import ALProxy
speak = ALProxy('ALTextToSpeech', '127.0.0.1', 9559)
audio = ALProxy('ALAudioDevice', '127.0.0.1', 9559)
audio.setOutputVolume(90)
    
import Queue
import motionInterface as mot
import visionInterface as vis
import gameStateController
import threading
import time
import math

# used for keeper
ball_loc = Queue.Queue(3)
ball_loc2 = Queue.Queue(3)
mean = ((0,0), (0,0))

# Initialization of archtitectuer
state = 0
phase = 'BallNotFound'

# PROXIES
leds = ALProxy('ALLeds', '127.0.0.1', 9559)
video = ALProxy('ALVideoDevice', '127.0.0.1', 9559)
video.setParam(18,1)

# Gamecontroller
sc = gameStateController.stateController('stateController')
gc = sc.gc #Do not use this for getting the game state, use sc instead

# robots own variables, taken from naoinfo.txt

robot = sc.getRobotNumber()
playerType = 0
if (robot == 0):
    playerType = 1
else:
    playerType = 0

# Try to connect to gameController, initialize (and change) variables used for kicking and other phases
teamColor = 0
kickOff = 0
penalty = 0

if gc.controlledRefresh():
    teamColor = gc.getTeamColor('we')
    kickOff = gc.getKickoff()
else:
    if sc.getSetup():
        (teamColor, penalty) = sc.setup(0, 0)
    
## MOTIONS FUNCTIONS
# normalPose()
# post.walkto()
# scanCircle()
# standup()
# rKickAngled()
# lKickAngled()
# hakje()
# FootRight()
# FootLeft()
# DiveRight()
# DiveLeft()
# stance()

## STATES (Gamestates)
# Initial()
# Ready()
# Set()
# Playing()
# Penalized()
# Finished()


# Initial state
# LEDS: All leds are off
# Motions: NormalPose
def Initial():
    global phase
    global ball_loc
    global ball_loc2
    while not(ball_loc.empty()):
        ball_loc.get()
    while not(ball_loc2.empty()):
        ball_loc2.get()    
    leds.off('AllLeds')
    mot.stiff()
    mot.killAllTasks()
    #print gc.controlledRefresh()
    if (teamColor == 0):
        leds.fadeRGB('RightFootLeds', 0x000000ff, 0)
        leds.fadeRGB('LeftFootLeds', 0x000000ff, 0)
    else:
        leds.fadeRGB('RightFootLeds', 0x00ff0000, 0)
        leds.fadeRGB('LeftFootLeds', 0x00ff0000, 0)
    
# Ready state
# LEDS:  Chest Blue
#        Feet  Teamcolor
def Ready():
    global phase
    if phase != 'BallNotFound' and phase != 'BallNotFoundKeep':
        if playerType == 0:
            mot.killWalk()
            phase = 'BallNotFound'
            mot.move('normalPose')
        else:
            mot.killWalk()
            phase = 'BallNotFoundKeep'
            mot.move('keepNormalPose')
    leds.fadeRGB('ChestLeds',0x000000ff, 0)

# Set state
# LEDS:  Chest Yellow
def Set():
    global phase
    global playerType
    if not mot.getNormalPose():
        mot.stiff()    
        mot.normalPose()
    leds.fadeRGB('ChestLeds',0x00ffff00, 0)
    # head movements allowed in the set state
    #  FIND A BALL  #
    if phase != 'BallFound' and phase != 'BallFoundKeep':
        if scanCircle():
            if playerType == 0:
                phase = 'BallFound'
            else:
                phase = 'BallFoundKeep'
        else:
            if playerType == 0:
                phase = 'BallNotFound'
            else:
                phase = 'BallNotFoundKeep'
    # keep finding the ball, it might move (or the nao moves)
    if not(vis.findBall()):
            if playerType == 0:
                phase = 'BallNotFound'
            else:
                phase = 'BallNotFoundKeep'
    audio.setOutputVolume(0)
        
# Playing state
# LEDS:  Chest Green
def Playing():
    global phase
    leds.fadeRGB('ChestLeds', 0x0000ff00, 0)
    # if nao has fallen down, stand up
    if mot.standUp():
        if playerType == 0:
            # and change phase to ballnotfound
            phase = 'BallNotFound'
        else:
            phase = 'BallNotFoundKeep'
    # execute the phase
    phases.get(phase)()

# Penalized state
# LEDS:  Chest Red
def Penalized():
    global phase
    if mot.isWalking():
        mot.killWalk()
        mot.killAllTasks()
    mot.stiff()
    leds.fadeRGB('ChestLeds',0x00ff0000, 0)
    if playerType == 0:
        phase = 'Unpenalized'
    else:
        if penalty:
            phase = 'BallNotFoundKeep'
        else:
            phase = 'Unpenalized'
        
def Finished():
    leds.off('AllLeds')
    if mot.isWalking():
        mot.killWalk()
    mot.stance()
    mot.setHead((0,0))
    time.sleep(1)
    mot.kill()
    #sc.close()
    audio.setOutputVolume(85)
## PHASES

# KEEPER
# BallFoundKeep()
# BallNotFoundKeep()
# BallApproaching()
# InGoalArea()

def BallFoundKeep():
    global phase
    global ball_loc
    global ball_loc2
    global mean
    
    #  FIND A BALL  #
    ball = vis.findBall()        
    if ball:
        #if ball[0] < 0.3 and (ball[1] < 0.5 or ball[1] > -0.5):
        #    phase = 'InGoalArea'
        #    mot.move('normalPose')
        #    vis.findBall()
        #    return True
        if ball_loc.empty():
            mot.move('stance')
        (x0,y0) = ball
        (ball_loc_mean, ball_loc2_mean) = mean
        (xnew, ynew) = ball_loc_mean    
        (xold, yold) = ball_loc2_mean
        
        # remove if too full 
        if ball_loc.full():
            (x3,y3) = ball_loc.get()
            xnew = (3.0 * xnew - x3 ) / 2.0
            ynew = (3.0 * ynew - y3 ) / 2.0
            
            # remove if too full 2
            if ball_loc2.full():
                (x6,y6) = ball_loc2.get()
                xold = (3.0 * xold - x6 ) / 2.0
                yold = (3.0 * yold - y6 ) / 2.0

            # update mean 2
            ball_loc2_len = ball_loc2.qsize()
            xold = (ball_loc2_len * xold + x3 ) / (ball_loc2_len+1)
            yold = (ball_loc2_len * yold + y3 ) / (ball_loc2_len+1)
            ball_loc2.put((x3,y3))

        # update mean            
        ball_loc_len = ball_loc.qsize()            
        xnew = (ball_loc_len * xnew + x0 ) / (ball_loc_len+1)
        ynew = (ball_loc_len * ynew + y0 ) / (ball_loc_len+1)
        ball_loc.put((x0,y0))
        
        ball_loc_mean = (xnew, ynew) 
        ball_loc2_mean = (xold, yold)
        mean = (ball_loc_mean, ball_loc2_mean) 
        
        # only after a certain number of measurements
        if ball_loc2.full():
        
            # calc diff in distance
            distold = math.sqrt(xold**2 + yold**2)
            distnew = math.sqrt(xnew**2 + ynew**2)
            speed = distold - distnew
            # print 'Ball moving from ', xold, yold, 'to', xnew, ynew, '(mean). Speed', speed
            # calculate direction
            if speed > 0.2:
                dir = (((yold - ynew )* xnew) / (xold - xnew)) - ynew
                while not(ball_loc.empty()):
                    ball_loc.get()
                ball_loc.put(dir)
                phase = 'BallApproaching'
                print 'Direction', dir
    else:
        phase = 'BallNotFoundKeep'
        
# Keeper lost track of ball
def BallNotFoundKeep():
    global phase
    global ball_loc
    global ball_loc2
    global mean
    
    # after x iterations of not seeing the ball
    if not ball_loc.empty(): 
        ball_loc.get()
    if not ball_loc2.empty():
        ball_loc2.get()
    # stand up
    if ball_loc.empty() and ball_loc.empty():
        mot.keepNormalPose()
        parallelToLine()
        mot.move('setHead', (0,0.4))
        angle, line = vis.pK()
        if (len(line) > 0):
            (x, y) = middleOfLine(line)
            print 'middleOfLine x,y:', x,y
            if(y>=50):
                mot.move('walkTo', (-0.01,0,0))
            if(y<=30):
                mot.move('walkTo', (0.01,0,0))
        goal = None   
        for angle in [-1,3, -0.85, 0.85, 1,3]:
            mot.move('setHeadBlock', (angle, -0.5, 0.4))
            #print 'Looking for goal in angle ', angle
            goal = vis.findGoal()
            if goal:
                break
        if(goal != None):
            
          
        
    
    # if a ball is found
    if scanCircle():
        # reinitialize
        mean = ((0,0),(0,0))
        while not(ball_loc.empty()):
            ball_loc.get()
        while not(ball_loc2.empty()):
            ball_loc2.get()
        phase = 'BallFoundKeep'
        
def middleOfLine(line):
    ((p1,p2),(p3,p4)) = line
    newP1 = (p1+p3)/2
    newP2 = (p2+p4)/2
    return (newP1, newP2)

def parallelToLine():
    if(scanLine()):
        angle, line = vis.pK()
        if(math.fabs(angle) >= 0.15): 
            mot.move('walkTo', (0,0,angle))
    else:
        print 'NO LINE FOUND'        
    

# Ball approaches keeper and goal area at considerable speed
def BallApproaching():
    global phase
    global ball_loc
    global ball_loc2
    dir = ball_loc.get()
    if dir >= 0.25:
        print 'Dive right'
        mot.diveRight()
    elif dir <= -0.25:
        print 'Dive left'
        mot.diveLeft()
    elif dir < 0.25 and dir >= 0:
        print 'Step right'
        mot.footRight()
    elif dir > -0.25 and dir < 0:
        print 'Step left'
        mot.footLeft()
    mot.keepNormalPose()
    phase = 'BallNotFoundKeep'
    
def InGoalArea():
    global phase
    global mean
    mot.move('normalPose')
    ball = vis.findBall()
    if ball:
        o_x = ball[0] - 0.15
        o_y = ball[1]
        theta = math.atan2(o_y, o_x)
    
        x = 0
        y = 0
        # walk towards the ball (this means it may not be moving. at all)
        while ball:
            ball = vis.findBall()
            if ball:
                (x,y) = ball
                print x,y
                if x < 0.19 and -0.04 > y > -0.08:
                    # BLOCKING CALL: FIND BALL WHILE STANDING STILL FOR ACCURATE RESULTS 
                    mot.killWalk()
                    ball = vis.findBall()
                    # correct one last time
                    if ball:
                        (x,y) = ball
                        mot.move('walkTo', (0, y + 0.04,0))
                        time.sleep(0.5)
                        print 'Kick'
                        mot.move('rKickAngled', 0)
                        time.sleep(1)
                        mot.move('normalPose')
                        break

                if x > 0.3 and (y > 0.2 or y < 0.2):
                    x = x * 2
                    y = y * 2
                if x > 1:
                    x = 1
                if y > 1:
                    y = 0.96
                elif y < -1:
                    y = -0.96
                if x >= -1:
                    mot.move('SWTV', (x-0.15,y+0.05,0, 1) )
            else:
                print 'ball lost, returning'
                
        mot.move('walkTo', ((x-0.15)-o_x, (y-0.04)-o_y, -theta))
    mot.keepNormalPose()
    phase = 'BallNotFoundKeep'

'''
def UnpenalizedKeep():
    global phase
    global ball_loc
    while  not( ball_loc.empty() ):
        ball_loc.get()
    ball_loc.put( (10, 10, 0) )
    phase = 'ReturnToGoal'

def ReturnToGoal():
    if mot.isWalking():
        return True
    
    # stored location of the goal (how to get there)
    (realX,realY,angle) = ball_loc.get()
    tempGoalPostBlue = None
    tempGoalPostYellow = None
    goal = None
    # color 0 is blue
    
    if realX > 0.75 or realY > 0.2 or realY < -0.2:
        for angle in [0, -0.5, 0.5, -1, 1, -1.5, 1.5, -2, 2]:
            mot.move('setHeadBlock', (angle, -0.55, 0.6))
            #print 'Looking for goal in angle ', angle
            goal = vis.findGoal(tempGoalPostBlue, tempGoalPostYellow)
            if goal:
                if goal[0] == 'Blue' and goal[1] == 'Post' and not tempGoalPostBlue:
                    tempGoalPostBlue = goal[2]
                elif goal[0] == 'Yellow' and goal[1] == 'Post' and not tempGoalPostYellow:
                    tempGoalPostYellow = goal[2]
                elif goal[1] == "Goal":
                    break
        if goal:
            #if (goal[0] == 'Blue' and teamColor == 1) or (goal[0] == 'Yellow' and teamColor == 0):
            #    turn around
            if type(goal[2]) == str or type(goal[2]) == dict:
                print 'saw', goal[2]
                mot.move('walkTo', (-0.2,0,0)) 
            else:
                leds.fadeRGB('LeftFaceLeds',0x0000ff00, 0) #goal, led turns green
                headangle = angle
                goal = goal[2] # info is stored in a tuple, 3d position
                realX = goal[0]
                realY = goal[1]
                angle = goal[2]
                
                # don't walk all the way
                if realX < 1.5:
                    x = realX / 2.0
                else:
                    x = realX
                if (0 < realY < 1.5) or (-1.5 < realY < 0):
                    y = realY / 2.0
                else:
                    y = realY
                mot.move('setHead', ( angle / 2.0, -0.55))
                mot.move('postWalkTo', (x / 2.0, y / 2.0, angle / 2.0))
                print goal
                

        else:
            mot.move('walkTo', (0, 0, 1))
            
    if realX < 0.75 and  -0.2 < realY < 0.2:
        if angle > 0:
            mot.move('walkTo', (0,realY, 3.1415 - goal[2]))
        else:
            mot.move('walkTo', (0,realY, 3.1415 + goal[2]))
        mot.move('walkTo',(0.1, 0, 0))
        mot.keepNormalPose()
        phase = "BallNotFoundKeep"
    ball_loc.put( (realX,realY,angle) )
'''
    
            
# PLAYER
# BallFound()
# BallNotFound()
# Kick()
# Unpenalized()
# ReturnField()

def BallFound():
    leds.fadeRGB('RightFaceLeds', 0x0000ff00, 0)
    global phase
    
    # FIND A BALL #
    ball = vis.findBall()
    if ball:
        (x,y) = ball
        print x,y
        if x < 0.225 and -0.02 < y < 0.02:
            print 'Kick'
            # BLOCKING CALL: FIND BALL WHILE STANDING STILL FOR ACCURATE CORRECTION
            mot.killWalk()
            time.sleep(0.2)
            phase = 'Kick'
            return True
    
        if x > 0.375 or y > 0.25 or y < -0.25:
            x = x * 2.5
            y = y * 1.5
            if x > 1:
                x = 1
            if y > 1:
                y = 1
            if y < -1:
                y = -1
            f = 1.0
        else:
            f = 0.9
        if x >= -1:
            mot.move('SWTV', (x-0.15,y,math.atan(y/x)/2.5, f) )

    else:
        print 'No Ball'
        mot.killWalk()
        phase = 'BallNotFound'

def BallNotFound():
    global ball_loc
    
    leds.fadeRGB('RightFaceLeds',0x00ff0000, 0) # no ball, led turns red
    global phase
    # try to find a ball
    if scanCircle():
        phase = 'BallFound'
    # if no ball is found
    else:
        # circle slowly
        mot.move('postWalkTo', (0, 0, 1.3))
        
def Kick():
    global phase
    leds.fadeRGB('RightFaceLeds',0x00000000, 0) # led turns black
    leds.fadeRGB('LeftFaceLeds', 0x00ff0000, 0) #no goal, led turns red
    
    # try different positions of the head
    goal = None   
    for angle in [-1, -0.5, 0, 0.5, 1]:
        mot.move('setHeadBlock', (angle, -0.5, 0.4))
        #print 'Looking for goal in angle ', angle
        goal = vis.findGoal()
        if goal:
            break
    
    
    # Case 0 : Ball stolen.
    # Check if the ball is still there
    mot.move('setHeadBlock', (0, 0.5, 0.2))
    time.sleep(0.3)
    vis.findBall()
    ball = vis.findBall()
    if not ball:
        print 'Ball gone'
        phase = 'BallNotFound'
        leds.fadeRGB('LeftFaceLeds',0x00000000, 0) # led turns black
        return False
    elif ball[0] > 0.25 or ball[1] > 0.1 or ball[1] < -0.1:
        print 'Ball too far'
        phase = 'BallFound'
        leds.fadeRGB('LeftFaceLeds',0x00000000, 0) # led turns black
        return False    
    elif ball and not goal:
        mot.move('walkTo', (0,0.04,0))
        mot.move('rKickAngled', 0)
        phase = 'BallNotFound'
        return False
    
    print "Kick phase:", goal

    # else a goal is found, together with it's color
    (color , kickangle ) = goal
    
    if color == 'Blue':
        goalColor = 0
    else:
        goalColor = 1
        
    # Cases 1-3, if you see your own goal, kick to the other side
    if goalColor == teamColor:
        # Case 1, goal is left, kick to the right. 
        if kickangle >= 0.7:
            kickangle = -1.2
        # Case 2, goal is right, kick to the left.
        if kickangle <= -0.7:
            kickangle = 1.2
        else:
        # Case 3, goal is straight forward, HAK
            mot.move('walkTo',(0,0.04,0))
            mot.move('normalPose')
            time.sleep(0.5)
            leds.fadeRGB('LeftFaceLeds',0x00000000, 0) # led turns black
            mot.move('hakje', kickangle * -0.1)
            phase = 'BallNotFound'
            return True
            
    # Case 4, other player's goal is found.
    # Kick towards it. 
    if kickangle > 1.2:
        kickangle = 1.2
    if kickangle < -1.2:
        kickangle = -1.2
    
    # conversion to real kickangle
    convert = 1.047 # (60 degrees rad)
    
    # kick either left or right
    if kickangle <= 0:
        mot.move('walkTo', (0, -0.04 + ball[1], 0))
        #strafe(-0.1 + ball[1])
        mot.move('lKickAngled', kickangle/ -convert)
    elif kickangle > 0:
        mot.move('walkTo', (0, 0.04 + ball[1], 0))
        #strafe(0.1 + ball[1])
        mot.move('rKickAngled', kickangle/ convert)
    leds.fadeRGB('LeftFaceLeds', 0x00000000, 0)
    phase = 'BallNotFound'

'''
def strafe(y, x):
    now = time.time()
    while time.time() - now < 2:
        if -1 < 2.5 * y < 1:
            mot.SWTV((-0.07, y + x * ball[1], -0.1, 1))
        else:
            mot.SWTV((-0.07, math.copysign(1, y), -0.1, 1))
    mot.SWTV((-0.0001,0,0,0.8))
    time.sleep(2)
    mot.SWTV((0,0,0,0))
'''
    
# the plan is, since you start at the side..
def Unpenalized():
    global phase
    # walk forward (2 meters)
    if not(mot.isWalking()):
        mot.move('postWalkTo', (2, 0, 0))
    phase = 'ReturnField'

# and while you're walking forward
def ReturnField():
    global phase
    if mot.isWalking():
        # search for a ball
        if scanCircle():
            # extra check to see if it's not incidental
            ball = vis.findBall()
            if ball:
                phase = 'BallFound'
    else:
        phase = 'BallNotFound'

# EXTRA FUNCTIONS
def scanCircle():            
    # extra check to stop the head from making unnecessary movements
    ball = vis.findBall()
    if ball:
        return ball
    for angles in [(-1.1, -0.4), (-0.75, -0.4), (-0.5, -0.4), (-0.25, -0.4), ( 0, -0.4), (0.25, -0.4), ( 0.5, -0.4), ( 1.1, -0.4), \
                   ( 1,  0.1  ), (0.75, 0.1), ( 0.5,  0.1  ), (0.25, 0.1), ( 0,  0.1  ), (-0.25, 0.1), (-0.5,  0.1  ), (-0.75, 0.1), (-1,  0.1  ), \
                   (-1,  0.4), (-0.5,  0.4),  ( 0,  0.5),  ( 0.5,  0.4),( 1,  0.4), (0,0) ]: # last one is to make successive scans easier
        mot.move('setHeadBlock', (angles[0], angles[1], 0.25))
        ball = vis.findBall()
        if ball:
            return ball
            
def scanLine():            
    # extra check to stop the head from making unnecessary movements
    line = vis.findLine()
    if (line):
        return line
    for angles in [( 1.2,  0.2  ), (0.8, 0.2), ( 0.5,  0.2  ), (0.25, 0.2), ( 0,  0.2  ), (-0.25, 0.2), (-0.5,  0.2  ), (-0.8, 0.2), (-1.2,  0.2  ), \
                   (-1.2,  0.4), (-0.5,  0.4),  ( 0,  0.4),  ( 0.5,  0.4),( 1.2,  0.4), (0,0) ]: # last one is to make successive scans easier
        mot.move('setHeadBlock', (angles[0], angles[1], 0.25))
        line = vis.findLine()
        if line:
            return line

# Constant updating the state
class stateUpdate(threading.Thread):
    def __init__(self, name):
        threading.Thread.__init__(self)
        self.name = name

    def run(self):
        global state
        while(1):
            state = sc.getState()
        print 'stopped'
update = stateUpdate("stateUpdate")

##### OPERATING SYSTEM #####
# The soul of the robot

def awakeSoul():
    global state
    sc.start()
    update.start()
    mot.stiff()
    while(1):
        print 'soul.py State: ', state
        states.get(state)()
    
    
# DICTIONARIES
# STATES
states =     {
    0: Initial,
    1: Ready,
    2: Set,
    3: Playing,
    4: Finished,
    10: Penalized
    }

# PHASES
phases =     {
    'BallFound': BallFound,
    'BallNotFound': BallNotFound,
    'Kick' : Kick,
    'BallFoundKeep': BallFoundKeep,
    'BallNotFoundKeep': BallNotFoundKeep,
    'BallApproaching': BallApproaching,
    'InGoalArea': InGoalArea,
    'Unpenalized': Unpenalized,
    'ReturnField': ReturnField
}

#awakeSoul()
